/**
 * @file App.c
 * @brief
 * @author He Shaohua (hsh15529870639@foxmail.com)
 * @date 2024-02-23
 * @version 1.0
 * @copyright Copyright (c) 2024, SNUT-Chaoyue Team
 * ****************************************************************************************************
 * @par Change Log:
 * <table>
 * <tr><th>Date         <th>Version       <th>Author      <th>Description
 * <tr><td>2023-10-03   <td> 1.0          <td>He Shaohua  <td>Creator
 *
 * </table>
 * =========================================----TASK LIST =============================================
 * <tr><th>Task Name                 <th>Priority                <th>Frequency/Hz       <th>Stack/Byte
 * <tr><td>Tes                       <td>osPriorityNormal        <td> NULL              <td>1500
 * <tr><td>                          <td>                        <td>                   <td>
 *
 *
 * *****************************************************************************************************
 * @attention
 *
 * This document is required and please understand any bugs that may occur in the document.
 * If you want to modify this file, after the code update is completed, update the version number in time,
 * indicate the update date, and fill in the changes
 * Make sure that the submitted code does not have many bugs.
 *
 * All rights reserved.
 * *****************************************************************************************************
 */
/* Includes Header -----------------------------------------------------------*/
#include "App.h"
#include "alg_pid.h"
#include "dev_encoder.h"
#include "dev_mpu.h"
/* Private Task handle ------------------------------------------------------*/
static TaskHandle_t KEYTask_Handle = NULL;
static TaskHandle_t MOTORTaskHandle = NULL;
static TaskHandle_t CONTROLTaskHandle = NULL;
static TaskHandle_t GRAYSTaskHandle = NULL;
static TaskHandle_t BEEPTaskHandle = NULL;
/* Private Kernel object handle ---------------------------------------------*/
/* Private global variables -------------------------------------------------*/
PID_Increase_Param_Config_Typedef pid_param[3] =
    {
        {.K_p = 6.0f, .K_i = 24.0f, .K_d = 0.0f, .i_limit = 100.0f, .out_limit_low = 0.0f, .out_limit_high = 500.0f},
        {.K_p = 6.0f, .K_i = 24.0f, .K_d = 0.0f, .i_limit = 100.0f, .out_limit_low = 0.0f, .out_limit_high = 500.0f},
        {.K_p = 2.0f, .K_i = 0.5f, .K_d = 0.0f, .i_limit = 100.0f, .out_limit_low = 0.0f, .out_limit_high = 500.0f}};
PID_Increase_Instance *Motor_L_pid = {0};
PID_Increase_Instance *Motor_R_pid = {0};
PID_Increase_Instance *Motor_Angle_pid = {0};
// float target_L = 145.0f, target_R = 100.0f;
float target_L = 0.0f, target_R = 0.0f;
uint8_t beep_flag = 0;
uint8_t Gray_sensor[7] = {0};
uint8_t times_4 = 0;
uint8_t direction_sclect = 0;
/* Private function prototypes ----------------------------------------------*/
void Turn_Left(void)
{
  if (Gray_sensor[0] == 1)
  {
    target_L = 60.0f;
    target_R = 120.0f;
  }
  else if (Gray_sensor[1] == 1)
  {
    target_L = 60.0f;
    target_R = 100.0f;
  }
  else if (Gray_sensor[2] == 1)
  {
    target_L = 60.0f;
    target_R = 90.0f;
  }
  else if (Gray_sensor[3] == 1)
  {
    target_L = 60.0f;
    target_R = 80.0f;
  }
  else if (Gray_sensor[4] == 1)
  {
    target_L = 60.0f;
    target_R = 100.0f;
  }
  else if (Gray_sensor[5] == 1)
  {
    target_L = 60.0f;
    target_R = 110.0f;
  }
  else if (Gray_sensor[6] == 1)
  {
    target_L = 60.0f;
    target_R = 120.0f;
  }
  else
  {
    target_L = 60.0f;
    target_R = 80.0f;
  }
}

void Turn_Right(void)
{
  if (Gray_sensor[0] == 1)
  {
    target_L = 130.0f;
    target_R = 60.0f;
  }
  else if (Gray_sensor[1] == 1)
  {
    target_L = 110.0f;
    target_R = 60.0f;
  }
  else if (Gray_sensor[2] == 1)
  {
    target_L = 100.0f;
    target_R = 60.0f;
  }
  else if (Gray_sensor[3] == 1)
  {
    target_L = 80.0f;
    target_R = 60.0f;
  }
  else if (Gray_sensor[4] == 1)
  {
    target_L = 110.0f;
    target_R = 60.0f;
  }
  else if (Gray_sensor[5] == 1)
  {
    target_L = 110.0f;
    target_R = 60.0f;
  }
  else if (Gray_sensor[6] == 1)
  {
    target_L = 120.0f;
    target_R = 60.0f;
  }
  else
  {
    target_L = 80.0f;
    target_R = 65.0f;
  }
}
void Turn_Left_Last(void)
{
  if (Gray_sensor[0] == 1)
  {
    target_L = 65.0f;
    target_R = 120.0f;
  }
  else if (Gray_sensor[1] == 1)
  {
    target_L = 65.0f;
    target_R = 100.0f;
  }
  else if (Gray_sensor[2] == 1)
  {
    target_L = 65.0f;
    target_R = 90.0f;
  }
  else if (Gray_sensor[3] == 1)
  {
    target_L = 65.0f;
    target_R = 70.0f;
  }
  else if (Gray_sensor[4] == 1)
  {
    target_L = 65.0f;
    target_R = 100.0f;
  }
  else if (Gray_sensor[5] == 1)
  {
    target_L = 65.0f;
    target_R = 110.0f;
  }
  else if (Gray_sensor[6] == 1)
  {
    target_L = 65.0f;
    target_R = 120.0f;
  }
//  else
//  {
//    target_L = 60.0f;
//    target_R = 70.0f;
//  }
}

void Turn_Right_Last(void)
{
  if (Gray_sensor[0] == 1)
  {
    target_L = 120.0f;
    target_R = 65.0f;
  }
  else if (Gray_sensor[1] == 1)
  {
    target_L = 110.0f;
    target_R = 65.0f;
  }
  else if (Gray_sensor[2] == 1)
  {
    target_L = 100.0f;
    target_R = 65.0f;
  }
  else if (Gray_sensor[3] == 1)
  {
    target_L = 87.0f;
    target_R = 65.0f;
  }
  else if (Gray_sensor[4] == 1)
  {
    target_L = 100.0f;
    target_R = 65.0f;
  }
  else if (Gray_sensor[5] == 1)
  {
    target_L = 110.0f;
    target_R = 65.0f;
  }
  else if (Gray_sensor[6] == 1)
  {
    target_L = 120.0f;
    target_R = 65.0f;
  }
  else
  {
    target_L = 87.0f;
    target_R = 65.0f;
  }
}

/* Private functions --------------------------------------------------------*/
void Robot_Init()
{
  NVIC_ClearPendingIRQ(GPIO_MULTIPLE_GPIOA_INT_IRQN);
  NVIC_EnableIRQ(GPIO_MULTIPLE_GPIOA_INT_IRQN);
  NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
  NVIC_EnableIRQ(IMU_INST_INT_IRQN);

  DL_TimerG_startCounter(PWM_MOTOR_INST);

  DL_GPIO_writePins(GPIOA, DL_GPIO_PIN_24);
  Motor_L_pid = PIDInit(&pid_param[0], NONE);
  Motor_R_pid = PIDInit(&pid_param[1], NONE);
  Motor_Angle_pid = PIDInit(&pid_param[2], NONE);
  DL_GPIO_clearPins(GPIOB, GPIO_GRP_0_PIN_LED_BIBI_PIN);
}
Task_List_Typedef task_list = Task_NULL; // 1，2，6，13
uint8_t Turn_flag = 0;
void Robot_Run_once(void)
{
//	DL_GPIO_writePins(GPIOB, GPIO_GRP_0_PIN_BIBI_PIN | GPIO_GRP_0_PIN_LED_BIBI_PIN);
  while (task_list == Task_NULL)
  {
    if (!DL_GPIO_readPins(GPIO_KEY_PORT, GPIO_KEY_KEY1_PIN))
    {
      task_list = Task_List_1;
    }

    if (!DL_GPIO_readPins(GPIO_KEY_PORT, GPIO_KEY_KEY2_PIN))
    {
      task_list = Task_List_2;
    }

    if (!DL_GPIO_readPins(GPIO_KEY_PORT, GPIO_KEY_KEY3_PIN))
    {
      task_list = Task_List_6;
    }

    if (!DL_GPIO_readPins(GPIO_KEY_PORT, GPIO_KEY_KEY4_PIN))
    {
      task_list = Task_List_13;
      
    }
  }
  TIMER_Delay_s(0.333);
}

uint8_t sensor_flag = 0;

/**
 * @brief 控制台
 *
 */
void CONTROLTask(void *pvParameters)
{
  float angle_inc = 0.0f;
  for (;;)
  {
    switch (task_list)
    {
      // **************************************** 第一题 **************************************************
    case Task_List_1:
      angle_inc -= PIDCalculate(Motor_Angle_pid, imu_yaw, 0.0f);
      target_L = 100.0f + angle_inc;
      target_R = 100.0f - angle_inc;
      target_L = 100.0f;
      target_R = 100.0f;
      if (Gray_sensor[0] == 1 || Gray_sensor[1] == 1 || Gray_sensor[2] == 1 ||
          Gray_sensor[3] == 1 || Gray_sensor[4] == 1 || Gray_sensor[5] == 1 || Gray_sensor[6] == 1)
      {
        beep_flag = 1;
        PIDReset(Motor_Angle_pid);
        angle_inc = 0.0f;
        target_L = 0.0f;
        target_R = 0.0f;
        task_list = Task_NULL;
      }
      break;

      // **************************************** 第二题 **************************************************
    case Task_List_2:
      angle_inc -= PIDCalculate(Motor_Angle_pid, imu_yaw, 0.0f);
      target_L = 100.0f + angle_inc;
      target_R = 100.0f - angle_inc;

      if (Gray_sensor[0] == 1 || Gray_sensor[1] == 1 || Gray_sensor[2] == 1 ||
          Gray_sensor[3] == 1 || Gray_sensor[4] == 1 || Gray_sensor[5] == 1 || Gray_sensor[6] == 1)
      {
        PIDReset(Motor_Angle_pid);
        beep_flag = 1;
        angle_inc = 0.0f;
        target_L = 100.0f;
        target_R = 75.0f;
        task_list = Task_List_3;
      }
      break;

    case Task_List_3:
      if (!direction_sclect)
      {
        Turn_Right();
      }
      else
      {
        Turn_Left();
      }
      if (Gray_sensor[0] == 0 && Gray_sensor[1] == 0 && Gray_sensor[2] == 0 && Gray_sensor[3] == 0 &&
          Gray_sensor[4] == 0 && Gray_sensor[5] == 0 && Gray_sensor[6] == 0 && imu_yaw < -160.0f)
      {
        sensor_flag++;
      }
      if (sensor_flag > 8)
      {
        beep_flag = 1;
        target_L = 0.0f;
        target_R = 0.0f;
        task_list = Task_List_4;
        sensor_flag = 0;
      }
      break;

    case Task_List_4:
      if (imu_yaw > 0.0f)
      {
        imu_yaw = imu_yaw - 360.0f;
      }
      angle_inc -= PIDCalculate(Motor_Angle_pid, imu_yaw, -180.0f);
      target_L = 100.0f + angle_inc;
      target_R = 100.0f - angle_inc;

      if (Gray_sensor[0] == 1 || Gray_sensor[1] == 1 || Gray_sensor[2] == 1 || Gray_sensor[3] == 1 ||
          Gray_sensor[4] == 1 || Gray_sensor[5] == 1 || Gray_sensor[6] == 1)
      {
        beep_flag = 1;
        PIDReset(Motor_Angle_pid);
        angle_inc = 0.0f;
        target_L = 75.0f;
        target_R = 75.0f;
        task_list = Task_List_5;
      }
      break;
    case Task_List_5:
      if (!direction_sclect)
      {
        Turn_Right();
      }
      else
      {
        Turn_Left();
      }
      if (Gray_sensor[0] == 0 && Gray_sensor[1] == 0 && Gray_sensor[2] == 0 && Gray_sensor[3] == 0 &&
          Gray_sensor[4] == 0 && Gray_sensor[5] == 0 && Gray_sensor[6] == 0 && (imu_yaw > -10.0f && imu_yaw < 10.0f))
      {
        sensor_flag++;
      }
      if (sensor_flag > 5)
      {
        beep_flag = 1;
        target_L = 0.0f;
        target_R = 0.0f;
        task_list = Task_NULL;
        sensor_flag = 0;
      }
      break;

      // **************************************** 第三题 **************************************************
    case Task_List_6:
      // 偏移角度为38.6598°
      angle_inc -= PIDCalculate(Motor_Angle_pid, imu_yaw, -38.66f);
      target_L = 100.0f + angle_inc;
      target_R = 100.0f - angle_inc;

      if (Gray_sensor[0] == 1 || Gray_sensor[1] == 1 || Gray_sensor[2] == 1 ||
          Gray_sensor[3] == 1 || Gray_sensor[4] == 1 || Gray_sensor[5] == 1 || Gray_sensor[6] == 1)
      {
        beep_flag = 1;
        PIDReset(Motor_Angle_pid);
        angle_inc = 0.0f;
        target_L = 0.0f;
        target_R = 50.0f;
        task_list = Task_List_7;
      }
      break;
    case Task_List_7:
      vTaskDelay(600);
      target_L = 0.0f;
      target_R = 50.0f;
      task_list = Task_List_8;
      break;
    case Task_List_8:
      Turn_Left_Last();

      if (Gray_sensor[0] == 0 && Gray_sensor[1] == 0 && Gray_sensor[2] == 0 && Gray_sensor[3] == 0 &&
          Gray_sensor[4] == 0 && Gray_sensor[5] == 0 && Gray_sensor[6] == 0 && imu_yaw > 160.0f)
      {
        sensor_flag++;
      }
      if (sensor_flag > 5)
      {
        beep_flag = 1;
        target_L = 0.0f;
        target_R = 50.0f;
        task_list = Task_List_9;
        sensor_flag = 0;
      }
      break;
    case Task_List_9:
      vTaskDelay(800);
      target_L = 50.0f;
      target_R = 0.0f;
      task_list = Task_List_10;
      break;

    case Task_List_10:
      angle_inc -= PIDCalculate(Motor_Angle_pid, imu_yaw, -145.0f);
      target_L = 100.0f + angle_inc;
      target_R = 100.0f - angle_inc;

      if (Gray_sensor[0] == 1 || Gray_sensor[1] == 1 || Gray_sensor[2] == 1 ||
          Gray_sensor[3] == 1 || Gray_sensor[4] == 1 || Gray_sensor[5] == 1 || Gray_sensor[6] == 1)
      {
        beep_flag = 1;
        PIDReset(Motor_Angle_pid);
        angle_inc = 0.0f;
        target_L = 50.0f;
        target_R = 0.0f;
        task_list = Task_List_11;
      }
      break;
    case Task_List_11:
      vTaskDelay(700);
      target_L = 50.0f;
      target_R = 50.0f;
      task_list = Task_List_12;
      break;
    case Task_List_12:
      Turn_Right_Last();

      if (Gray_sensor[0] == 0 && Gray_sensor[1] == 0 && Gray_sensor[2] == 0 && Gray_sensor[3] == 0 &&
          Gray_sensor[4] == 0 && Gray_sensor[5] == 0 && Gray_sensor[6] == 0 && (imu_yaw > -12.0f && imu_yaw < 12.0f))
      {
        sensor_flag++;
      }
      if (sensor_flag > 10)
      {
        beep_flag = 1;
        target_L = 0.0f;
        target_R = 0.0f;
        task_list = Task_NULL;
        sensor_flag = 0;
      }
      break;
    // **************************************** 第四题 **************************************************
    case Task_List_13:
      // 偏移角度为38.6598°
      angle_inc -= PIDCalculate(Motor_Angle_pid, imu_yaw, -38.66f);
      target_L = 100.0f + angle_inc;
      target_R = 100.0f - angle_inc;

      if (Gray_sensor[0] == 1 || Gray_sensor[1] == 1 || Gray_sensor[2] == 1 ||
          Gray_sensor[3] == 1 || Gray_sensor[4] == 1 || Gray_sensor[5] == 1 || Gray_sensor[6] == 1)
      {
        beep_flag = 1;
        PIDReset(Motor_Angle_pid);
        angle_inc = 0.0f;
        target_L = 0.0f;
        target_R = 50.0f;
        task_list = Task_List_14;
      }
      break;
    case Task_List_14:
      vTaskDelay(600);
      target_L = 0.0f;
      target_R = 50.0f;
      task_list = Task_List_15;
      break;
    case Task_List_15:
      Turn_Left_Last();

      if (Gray_sensor[0] == 0 && Gray_sensor[1] == 0 && Gray_sensor[2] == 0 && Gray_sensor[3] == 0 &&
          Gray_sensor[4] == 0 && Gray_sensor[5] == 0 && Gray_sensor[6] == 0 && imu_yaw > 160.0f)
      {
        sensor_flag++;
      }
      if (sensor_flag > 5)
      {
        beep_flag = 1;
        target_L = 0.0f;
        target_R = 50.0f;
        task_list = Task_List_16;
        sensor_flag = 0;
      }
      break;
    case Task_List_16:
      vTaskDelay(800);
      target_L = 50.0f;
      target_R = 0.0f;
      task_list = Task_List_17;
      break;

    case Task_List_17:
      angle_inc -= PIDCalculate(Motor_Angle_pid, imu_yaw, -142.0f);
      target_L = 100.0f + angle_inc;
      target_R = 100.0f - angle_inc;

      if (Gray_sensor[0] == 1 || Gray_sensor[1] == 1 || Gray_sensor[2] == 1 ||
          Gray_sensor[3] == 1 || Gray_sensor[4] == 1 || Gray_sensor[5] == 1 || Gray_sensor[6] == 1)
      {
        beep_flag = 1;
        PIDReset(Motor_Angle_pid);
        angle_inc = 0.0f;
        target_L = 50.0f;
        target_R = 0.0f;
        task_list = Task_List_18;
      }
      break;
    case Task_List_18:
      vTaskDelay(700);
      target_L = 50.0f;
      target_R = 50.0f;
      task_list = Task_List_19;
      break;
    case Task_List_19:
      Turn_Right_Last();

      if (Gray_sensor[0] == 0 && Gray_sensor[1] == 0 && Gray_sensor[2] == 0 && Gray_sensor[3] == 0 &&
          Gray_sensor[4] == 0 && Gray_sensor[5] == 0 && Gray_sensor[6] == 0 && (imu_yaw > -10.0f && imu_yaw < 10.0f))
      {
        sensor_flag++;
      }
      if (sensor_flag > 10)
      {
        beep_flag = 1;
        target_L = 0.0f;
        target_R = 0.0f;
        task_list = Task_List_20;
        sensor_flag = 0;
      }
      break;

    case Task_List_20:
      if (times_4 > 2)
      {
        task_list = Task_NULL;
      }
      else
      {
        task_list = Task_List_13;
        times_4++;
      }
      vTaskDelay(200);
      break;

    default:
      break;
    }
    vTaskDelay(40);
  }
}

/**
 * @brief 电机控制任务
 */
void MOTORTask(void *pvParameters)
{
  float last_L = 0.0f;
  float last_R = 0.0f;
  float Cal_Duty_Motor_L = 0.0f;
  float Cal_Duty_Motor_R = 0.0f;

  for (;;)
  {
    Speed_L = 0.5f * Speed_L + 0.5 * last_L;
    Cal_Duty_Motor_L += PIDCalculate(Motor_L_pid, Speed_L, target_L);
    DL_TimerG_setCaptureCompareValue(PWM_MOTOR_INST, Cal_Duty_Motor_L, GPIO_PWM_MOTOR_C1_IDX);
    last_L = Speed_L;

    Speed_R = 0.5f * Speed_R + 0.5 * last_R;
    Cal_Duty_Motor_R += PIDCalculate(Motor_R_pid, Speed_R, target_R);

    DL_TimerG_setCaptureCompareValue(PWM_MOTOR_INST, Cal_Duty_Motor_R, GPIO_PWM_MOTOR_C0_IDX);
    last_R = Speed_R;
    vTaskDelay(50);
  }
}

/**
 * @brief 灰度传感器
 *
 */

void GRAYSTask(void *pvParameters)
{
  for (;;)
  {
    // 1号脚
    if (!DL_GPIO_readPins(GPIOA, GRP_Trace_PIN_1_PIN)) // 白色低电平
    {
      Gray_sensor[0] = 0;
    }
    else
    {
      Gray_sensor[0] = 1;
    }

    // 2号脚
    if (!DL_GPIO_readPins(GPIOA, GRP_Trace_PIN_2_PIN)) // 白色低电平
    {
      Gray_sensor[1] = 0;
    }
    else
    {
      Gray_sensor[1] = 1;
    }

    // 3号脚
    if (!DL_GPIO_readPins(GPIOA, GRP_Trace_PIN_3_PIN)) // 白色低电平
    {
      Gray_sensor[2] = 0;
    }
    else
    {
      Gray_sensor[2] = 1;
    }

    // 4号脚
    if (!DL_GPIO_readPins(GPIOA, GRP_Trace_PIN_4_PIN)) // 白色低电平
    {
      Gray_sensor[3] = 0;
    }
    else
    {
      Gray_sensor[3] = 1;
    }

    // 5号脚
    if (!DL_GPIO_readPins(GPIOA, GRP_Trace_PIN_5_PIN)) // 白色低电平
    {
      Gray_sensor[4] = 0;
    }
    else
    {
      Gray_sensor[4] = 1;
    }

    // 6号脚
    if (!DL_GPIO_readPins(GPIOA, GRP_Trace_PIN_6_PIN)) // 白色低电平
    {
      Gray_sensor[5] = 0;
    }
    else
    {
      Gray_sensor[5] = 1;
    }

    // 7号脚
    if (!DL_GPIO_readPins(GPIOA, GRP_Trace_PIN_7_PIN)) // 白色低电平
    {
      Gray_sensor[6] = 0;
    }
    else
    {
      Gray_sensor[6] = 1;
    }
    vTaskDelay(30);
  }
}

/**
 * @brief 灰度传感器
 *
 */

void BEEPTask(void *pvParameters)
{
  for (;;)
  {
    if (beep_flag)
    {
      DL_GPIO_writePins(GPIOB, GPIO_GRP_0_PIN_BIBI_PIN | GPIO_GRP_0_PIN_LED_BIBI_PIN);
      //		DL_GPIO_writePins(GPIOB,GPIO_GRP_0_PIN_LED_BIBI_PIN);
      vTaskDelay(500);
      DL_GPIO_clearPins(GPIOB, GPIO_GRP_0_PIN_BIBI_PIN | GPIO_GRP_0_PIN_LED_BIBI_PIN);
      //	DL_GPIO_clearPins(GPIOB,GPIO_GRP_0_PIN_LED_BIBI_PIN);
      beep_flag = 0;
    }
    vTaskDelay(10);
  }
}
/**
 * @brief 任务栈为10240Bytes
 * @note MINIMAL_STACK_SIZE 任务栈为128
 *
 */
void Robot_Run(void)
{
  BaseType_t xReturn = pdPASS;

  taskENTER_CRITICAL();

  /* 创建CONTROLTask任务 */
  xReturn = xTaskCreate((TaskFunction_t)CONTROLTask,
                        (const char *)"CONTROLTask",
                        (uint16_t)888,
                        (void *)NULL,
                        (UBaseType_t)15,
                        (TaskHandle_t *)&CONTROLTaskHandle);

  /* 创建MOTORTask任务 */
  xReturn = xTaskCreate((TaskFunction_t)MOTORTask,
                        (const char *)"MOTORTask",
                        (uint16_t)512,
                        (void *)NULL,
                        (UBaseType_t)15,
                        (TaskHandle_t *)&MOTORTaskHandle);

  /* 创建GRAYSTask任务 */
  xReturn = xTaskCreate((TaskFunction_t)GRAYSTask,
                        (const char *)"GRAYSTask",
                        (uint16_t)256,
                        (void *)NULL,
                        (UBaseType_t)15,
                        (TaskHandle_t *)&GRAYSTaskHandle);

  /* 创建BEEPSTask任务 */
  xReturn = xTaskCreate((TaskFunction_t)BEEPTask,
                        (const char *)"BEEPTask",
                        (uint16_t)128,
                        (void *)NULL,
                        (UBaseType_t)15,
                        (TaskHandle_t *)&BEEPTaskHandle);
  taskEXIT_CRITICAL();
}

/**
 * @}
 */

/**
 * @}
 */

/**
 * @}
 */

/*********************************END OF FILE*********************************/